/*
 * URFLocalize.c
 *
 *  Created on: Nov 7, 2012
 *      Author: gmkado
 */
#include "URF.h"
#include "URFLocalize.h"
#include <math.h>
#include <stdio.h>
#include "rit128x96x4.h"

int numSensors = 3;

/********** LAYOUT MEASUREMENTS ********/
//sensor positions on axis
float Lx = 1; // relative position of coil in car's x
float Ly = 0.5; // relative position of coil in car's y

float x1 = 1; // xpos of sensor 1
float y1 = 1; // ypos of sensor 2
float y2 = 2; // ypos of sensor 3

/**************************************/

// get each range and display on OLED
void displayAllRanges(void)
{
	//char message[20];
	// loop through ID's
	char i;
	printf("\r");
	for(i=0; i<numSensors; i++){
		displayURFRange(i);
	}
}

// return whether a car is detected or not
char isCarPresent(void)
{
	//TODO: FILL THIS OUT
	return 0;
}

// return whether a car is in a valid position or not
char isCarValid(void)
{
	//TODO: FILL THIS OUT
	return 0;
}

/* getLocation
 * d1 = side URF 1
 * d2 = side URF 2
 * d3 = front URF
 assign floats to input array of the car's (x,y,theta) position */
void getLocation(float *loc)
{

	float dist[numSensors];

	// get all distance readings
	int i;
	for (i = 0; i<numSensors; i ++)
	{
		dist[i] = getURFRange(i);
	}

	// TODO: case for when dist[1] = dist[0]
	float m = (y2-y1)/(dist[1]-dist[0]); // y = mx + b
    float b = y2 - (y2-y1)/(dist[1]-dist[0]) * dist[1];
    float c = dist[2] + (dist[1]-dist[0])/(y2-y1) * x1; // y_perp = -x/m + c

    // get the car corner point
    float x0 = (c-b)*m/(pow(m,2)+1);
    float y0 = (pow(m,2)*c+b)/(pow(m,2)+1);

    // get the car tilt angle
    loc[2] = atan((dist[1]-dist[0])/(y2-y1));

    // get location of secondary coil
    loc[0] = x0 + Lx*cos(loc[2]) + Ly*sin(loc[2]);
    loc[1] = y0 - Lx*sin(loc[2]) + Ly*cos(loc[2]);

}
